/**
 * Tasks
 * Move to exact height (neutral, top, bottom)
 * Self calibrate
 */
package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.*;
import java.util.TimerTask;

/**
 *
 * @author Steve
 */
public class Lifter extends TimerTask {

    Joystick joyStick;
    
    // 0 volts is on -- 5 volts is off
    AnalogChannel topAnalogChannel;
    AnalogChannel btmAnalogChannel;
    
    AnalogTrigger topTrigger;
    AnalogTrigger btmTrigger;

    AnalogTriggerOutput topOutput;
    AnalogTriggerOutput btmOutput;

    Boolean prevTopChannelState = null;
    Boolean prevBtmChannelState = null;

    AnalogLimitSwitch topSwitch = new AnalogLimitSwitch( topAnalogChannel);
    AnalogLimitSwitch btmSwitch = new AnalogLimitSwitch( btmAnalogChannel);

    Victor lifterMotor;
    private int CRIO_SLOT = Constants.CRIO_ANALOG_BREAKOUT;
    private int LIFTER_TOP_LIMIT_SWITCH = Constants.LIFTER_TOP_LIMIT_SWITCH;
    private int LIFTER_BOTTOM_LIMIT_SWITCH = Constants.LIFTER_BOTTOM_LIMIT_SWITCH;
    private int LIFTER_MOTOR = Constants.LIFTER_MOTOR;
    private final int BOTTOM = -1;
    private final int NEUTRAL = 0;
    private final int TOP = 1;
    private final int GO_UP = 1;
    private final int GO_DOWN = -1;
    private int position;
    private boolean calibrated;

    public Lifter(Joystick joyStick) {
        System.out.println("Lifter::Lifter");
        this.joyStick = joyStick;
        calibrated = false;
        position = NEUTRAL;

        topAnalogChannel = new AnalogChannel(CRIO_SLOT, LIFTER_TOP_LIMIT_SWITCH);
        btmAnalogChannel = new AnalogChannel(CRIO_SLOT, LIFTER_BOTTOM_LIMIT_SWITCH);

        AnalogLimitSwitch topSwitch = new AnalogLimitSwitch( topAnalogChannel);
        AnalogLimitSwitch btmSwitch = new AnalogLimitSwitch( btmAnalogChannel);


//       topTrigger = new AnalogTrigger(topSwitch);
//       btmTrigger = new AnalogTrigger(bottomSwitch);

//         AnalogTriggerOutput.Type outputType = AnalogTriggerOutput.Type.kTypeState;
//         topTrigger = new AnalogTrigger(CRIO_SLOT, LIFTER_TOP_LIMIT_SWITCH);
//         btmTrigger = new AnalogTrigger(CRIO_SLOT, LIFTER_BOTTOM_LIMIT_SWITCH);
//
//         topOutput = new AnalogTriggerOutput(topTrigger, outputType );
//         btmOutput = new AnalogTriggerOutput(topTrigger, outputType );
//
//         topTrigger.setLimitsVoltage( 2.0, 3.0 );
//         btmTrigger.setLimitsVoltage( 2.0, 3.0 );
//
//         lifterMotor = new Victor(Constants.CRIO_DIGITAL_SIDECAR, LIFTER_MOTOR);
        //Set to calibration mode and let run until it gets there. Won't start
        //moving until it becomes enabled
    }

    public void run() {
        //Be sure to wait until calibration is finished before allowing arm
        //to do anything!
        System.out.println("Lifter::run");

        Boolean top = Boolean.valueOf( readTopSwitch() );
        Boolean bottom = Boolean.valueOf( readBottomSwitch() );
//        System.out.println( "Top:" + top.toString() );
//        System.out.println( "Btm:" + bottom.toString() );
        SmartDashboard.log(top.toString(), "Top Limit");
        SmartDashboard.log(bottom.toString(), "Btm Limit");
        
        if (!calibrated) {
            if (!readTopSwitch() ) {
                //lifterMotor.set(TOP);
                lifterTop();
                return;
            } else {
                calibrated = true;
            }
        }

        //Check to see if a button is pressed. If it is, figure out what level
        //to move it to. Set PID to go to that height.
        //Remember to consider that a joystick button may be pressed through
        //multiple cycles. Won't necessarily be a single press in a single
        //iterationlifterUp
        boolean lifterUp = true;
        if (isButtonPressed(Constants.BTN_FLOOR)) {
            lifterUp = true;
        } else if (isButtonPressed(Constants.BTN_TUCKED)) {
            lifterUp = false;
        } else if (isButtonPressed(Constants.BTN_BOTTOM_ROW_SIDE)) {
            lifterUp = false;
        } else if (isButtonPressed(Constants.BTN_BOTTOM_ROW_CENTER)) {
            lifterUp = false;
        } else if (isButtonPressed(Constants.BTN_MIDDLE_ROW_SIDE)) {
            lifterUp = true;
        } else if (isButtonPressed(Constants.BTN_MIDDLE_ROW_CENTER)) {
            lifterUp = true;
        } else if (isButtonPressed(Constants.BTN_TOP_ROW_SIDE)) {
            lifterUp = true;
        } else if (isButtonPressed(Constants.BTN_TOP_ROW_CENTER)) {
            lifterUp = true;
        } else {
            // no buttons pressed - don't move lifter
            return;
        }

//        if (lifterUp) {
//            lifterTop();
//        } else {
//            lifterBottom();
//        }
    }

    // Action for LIFTER_BUTTON_TOP
    private void lifterTop() {
        if (position != TOP) {
            // Not at the top yet
            position = NEUTRAL;
            if (readTopSwitch()) {
                // We hit top -- turn off motor
                position = TOP;
                lifterMotor.set(0);
            } else {
                // Still not there yet
                lifterMotor.set(GO_UP * Constants.LIFTER_RATE);
            }
        }
    }

    // Action for LIFTER_BUTTON_BOTTOM
    private void lifterBottom() {
        if (position != BOTTOM) {
            position = NEUTRAL;
            // Move to bottom
            if (readBottomSwitch()) {
                // We hit bottom -- turn off motor
                position = BOTTOM;
                lifterMotor.set(0);
            } else {
                // Still not there yet
                lifterMotor.set(GO_DOWN * Constants.LIFTER_RATE);
            }
        }
    }

    private boolean readTopSwitch( )
    {
        return topSwitch.readSwitch();
    }

    private boolean readBottomSwitch( )
    {
        return btmSwitch.readSwitch();
    }

    private boolean isButtonPressed(int button) {
        // if more than one button - return false
        if (buttonCount() > 1) {
            return false;
        } else {
            return joyStick.getRawButton(button);
        }
    }

    private int buttonCount() {
        int count = 0;
        if (isButtonPressed(Constants.BTN_FLOOR)) {
            count++;
        } else if (isButtonPressed(Constants.BTN_TUCKED)) {
            count++;
        } else if (isButtonPressed(Constants.BTN_BOTTOM_ROW_SIDE)) {
            count++;
        } else if (isButtonPressed(Constants.BTN_BOTTOM_ROW_CENTER)) {
            count++;
        } else if (isButtonPressed(Constants.BTN_MIDDLE_ROW_SIDE)) {
            count++;
        } else if (isButtonPressed(Constants.BTN_MIDDLE_ROW_CENTER)) {
            count++;
        } else if (isButtonPressed(Constants.BTN_TOP_ROW_SIDE)) {
            count++;
        } else if (isButtonPressed(Constants.BTN_TOP_ROW_CENTER)) {
            count++;
        }
        return count;
    }

    // True if above upper limit. False if below lower limit.
    // ours are true for low voltage -- false high voltage
//    private boolean readSwitch( AnalogTriggerOutput output )
//    {
//        boolean triggerState = output.get();
//        return !triggerState;
//    }


//            boolean prevTopChannelState = false;
//        boolean prevBtmChannelState = false;
//    private Boolean readSwitch( AnalogChannel channel )
//    {
//        double voltage = channel.getVoltage();
//        Boolean retval = null;
//        if ( voltage < 2.0 )
//        {
//            retval = Boolean.TRUE;
//        }
//        else if ( voltage >3.0 )
//        {
//            retval = Boolean.FALSE;
//        }
//        return retval;
//    }

//    private boolean readTopSwitch( )
//    {
//        Boolean  value = readSwitch(topAnalogChannel);
//        if ( value != null )
//        {
//            prevTopChannelState = value;
//        }
//        return prevTopChannelState.booleanValue();
//    }
//
//    private boolean readBottomSwitch( )
//    {
//        Boolean  value = readSwitch(btmAnalogChannel);
//        if ( value != null )
//        {
//            prevBtmChannelState = value;
//        }
//        return prevBtmChannelState.booleanValue();
//    }
}

